The present invention relates generally to an inertial navigation system wherein sensors such as a gyroscope and an accelerometer are provided on each of axes extended in at least four directions defined from a certain point to form a redundancy inertial sensor system and information pulses from each sensor are used to compute the velocity, position and attitude angle of an inertial object. More particularly, the invention pertains to an apparatus for correcting dynamic errors in such an inertial navigation system.
In a conventional strap down inertial navigation system, for example, as shown in FIG. 3, sensors are each provided on one of three axes (X, Y and Z axes) intersecting perpendicularly to one another and a skew axis (an S axis) diagonal to them to form a redundancy inertia sensor system. In this instance, if the S axis is selected such that it is spaced the same angle (.theta.s) apart from any of the X, Y and Z axes, then the angle .theta.s will have a value which satisfies ##EQU1##
Sensors such as a gyroscope and an accelerometer are provided for each of the four axes so that data can be obtained for each axis, but information on the inertial object, such as its velocity, position and attitude angle, can be obtained with data from respective sensors in three of the four axes; in general, such information is obtained from the sensors of the three perpendicularly intersecting axes (X, Y and Z). With such a four-axis structure, even if failures occur in the sensors in one of the three axes, for example, in the Z axis, data from the S-axis sensors are converted into data equivalent to those from the Z-axis sensors so that necessary information can still be obtained as data from the sensors in the perpendicularly intersecting axes, and hence computations for navigation can be continued. The prior art utilizes this navigation system.
In the case of constituting sensor systems by combinations of three axes out of the four axes, a total of four sensor systems (X, Y, Z), (X, Y, S), (Y, Z, S) and (X, Z, S) are obtainable, and the necessary inertial object information is normally derived from information which is available from the sensors of the (X, Y, Z) system. In the case of the (Y, Z, S)-axis sensor system, data from the Y-, Z- and S-axis sensors are used to calculate data equivalent to those from the X-axis sensor, and in the case of the (X, Z, S)-axis sensor system, data from the X-, Z- and S-axis sensors are used to calculate data equivalent to those from the Y-axis sensor.
Since the data available from each sensor system are processed on a discrete-time basis, a motion accompanied by rotation usually develops a dynamic error except in specific cases. This error is a coning error in regard to the gyro output and a sculling error in regard to the accelerometer output. Computations for correcting such dynamic errors in the above-mentioned four sensor systems are expressed as listed below in the case of using linear expansions. In this instance, the dynamic error correcting computation is performed every preset computation cycle on the basis of the number of pulses which are provided from each sensor system during the period. Incidentally, respective symbols are explained below and suffixes, x, y, z and s indicate the axes concerned, respectively.
.delta..theta..sub.xn, .delta..theta..sub.yn, .delta..theta..sub.zn : Integrated value of the correctional quantity of a coning error in the current computation cycle regarding the direction of rotation in each axis; PA0 .delta..theta..sub.xn-1, .delta..theta..sub.yn-1, .delta..theta..sub.zn-1 : Integrated value of the correctional quantity of a coning error in the previous computation cycle regarding the direction of rotation in each axis; PA0 .DELTA..theta..sub.xn =.omega..sub.x, .DELTA..theta..sub.yn =.omega..sub.y, .DELTA..theta..sub.zn =.omega..sub.z, .DELTA..theta..sub.sn =.omega..sub.s : Number of gyro output pulses counted in the current computation cycle regarding the direction of rotation in each axis; PA0 .theta..sub.xn-1 =.phi..sub.x, .theta..sub.yn-1 =.phi..sub.y, .theta..sub.zn-1 =.phi..sub.z, .theta..sub.sn-1 =.phi..sub.s : Accumulated number of gyro output pulses counted until the previous computation cycle regarding the direction of rotation in each axis; PA0 .delta.V.sub.xn, .delta.V.sub.yn, .delta.V.sub.zn : Integrated value of the correctional quantity of a sculling error in the current computation cycle regarding the direction of rotation in each axis; PA0 .delta.V.sub.xn-1, .delta.V.sub.yn-1, .delta.V.sub.zn-1 : Integrated value of the correctional quantity of a sculling error in the previous computation cycle regarding the direction of rotation in each axis; PA0 .DELTA.V.sub.xn =A.sub.x, .DELTA.V.sub.yn =A.sub.y, .DELTA.V.sub.zn =A.sub.z, .DELTA.V.sub.sn =A.sub.s : Number of accelerometer output pulses in the current computation cycle regarding the direction of rotation in each axis.
Incidentally, it is also possible to use .theta..sub.xn, .theta..sub.yn, .theta..sub.zn (the numbers of gyro output pulses counted in the current computation cycle regarding the direction of rotation in the respective axes) for .theta..sub.xn-1, .theta..sub.yn-1, .theta..sub.zn-1. The symbols .phi..sub.x, .phi..sub.y, .phi..sub.s, .omega..sub.x, .omega..sub.y, .omega..sub.z, A.sub.x, A.sub.y, A.sub.z and A.sub.s in the above have been introduced to simplify expressions in the following description and in the drawings.
1. Correctional quantity of a coning error in the (X, Y, Z)-axis sensor system: ##EQU2##
Expressions (1x), (1y) and (1z) represent recurrence formulae of the accumulations of .delta..theta..sub.xn, .delta..theta..sub.yn and .delta..theta..sub.zn, respectively. Let it be assumed that when n=1, the first term .delta..theta..sub.x0, .delta..theta..sub.y0 and .delta..theta..sub.z0 on the right sides of the above formulae are each zero. .phi..sub.x, .phi..sub.y, .phi..sub.z, .omega..sub.x, .omega..sub.y and .omega..sub.z are updated every 0.1 msec, for example, and by repeatedly performing the recurrence formulae (1x), (1y) and (1z) for 20 msec, that is, by accumulating .delta..theta..sub.xn, .delta..theta..sub.yn and .delta..sub.zn from n=1 to 200, the correctional quantities which are given by the following expressions are obtained. EQU .SIGMA..delta..theta..sub.xn =.SIGMA.{.phi..sub.y .omega..sub.z -.phi..sub.z .omega..sub.y }=.SIGMA..DELTA.C.sub.A =C.sub.A( 1a) EQU .SIGMA..delta..theta..sub.yn =.SIGMA.{.phi..sub.z .omega..sub.x -.phi..sub.x .omega..sub.z }=.SIGMA..DELTA.C.sub.B =C.sub.B( 1b) EQU .SIGMA..delta..theta..sub.zn =.SIGMA.{.phi..sub.x .omega..sub.y -.phi..sub.y .omega..sub.x }=.SIGMA..DELTA.C.sub.C =C.sub.C( 1c)
2. Correctional quantity of a sculling error in the (X, Y, Z)-axis sensor system: ##EQU3## The correctional quantities which are calculated from the recurrence formulae (2x), (2y) and (2z) are given by the following expressions: EQU .SIGMA..delta.V.sub.xn =.SIGMA.{.phi..sub.y A.sub.z -.phi..sub.z A .sub.y }=.SIGMA..DELTA.S.sub.A =S.sub.A ( 2a) EQU .SIGMA..delta.V.sub.yn =.SIGMA.{.phi..sub.z A.sub.x -.phi..sub.x A.sub.z }=.SIGMA..DELTA.S.sub.B =S.sub.B ( 2b) EQU .SIGMA..delta.V.sub.zn =.SIGMA.{.phi..sub.x A.sub.y -.phi..sub.y A.sub.x }=.SIGMA..DELTA.S.sub.C =S.sub.C ( 2c)
Incidentally, the recurrence formulae (1x), (1y) and (1z) will hereinafter be generically called a formula (1) and the recurrence formulae (2x), (2y) and (2z) will be generically called a formula (2). Formulae in the following description will also be called similarly.
3. Correctional quantity of a coning error in the (X, Y, S)-axis sensor system: ##EQU4## The correctional quantities which are calculated from the recurrence formulae (3x), (3y) and (3z) are given by the following expressions: ##EQU5##
4. Correctional quantity of a sculling error in the (X, Y, S)-axis sensor system: ##EQU6## The correctional quantities which are calculated from the recurrence formulae (4x), (4y) and (4z) are given by the following expressions: ##EQU7##
5. Correctional quantity of a coning error in the (X, Z, S)-axis sensor system: ##EQU8## The correctional quantities which are calculated from the recurrence formulae (5x), (5y) and (5z) are given by the following expressions: ##EQU9##
6. Correctional quantity of a sculling error in the (X, Z, S)-axis sensor system: ##EQU10## The correctional quantities which are calculated from the recurrence formulae (6x), (6y) and (6z) are given by the following expressions: ##EQU11##
7. Correctional quantity of a coning error in the (X, Z, S)-axis sensor system: ##EQU12## The correctional quantities which are calculated from the recurrence formulae (7x), (7y) and (7z) are given by the following expressions: ##EQU13##
8. Correctional quantity of a sculling error in the (X, Z, S)-axis sensor system: ##EQU14## The correctional quantities which are calculated from the recurrence formulae (8x), (8y) and (8z) are given by the following expressions: ##EQU15## where the following physical formulae are used. ##EQU16##
In the inertial navigation system, calculations for the velocity, position and attitude of the inertial object are usually updated with a period of about 20 msec (50 Hz) to 10 msec (100 Hz). In the inertial navigation system employing the gyro, since the frequency of dynamic noise by the gyro is hundreds of hertz, the dynamic error needs to be corrected with a period of 0.1 msec or so in the case of utilizing the above recurrence formulae.
The arrangement for correcting the dynamic error has such a construction as shown in FIGS. 4 and 5 for the formulae (1) and (2) corresponding to the (X, Y, Z)-axis sensor system (FIG. 4 showing one half of the arrangement and FIG. 5 the other half, FIGS. 4 and 5 being connected by reference character B) and such a construction as shown in FIGS. 6 and 7 for the formulae (7) and (8) corresponding to the (Y, Z, S)-axis sensor system (FIG. 6 showing one half of the arrangement and FIG. 7 the other half, FIGS. 6 and 7 being connected by a reference character C). Though not shown, the conventional apparatus further includes similar arrangements for performing the dynamic error calculations of the formulae (3) and (4) corresponding to the (X, Y, S)-axis sensor system and the formulae (5) and (6) corresponding to the (X, Z, S)-axis sensor system. Shown below each block is the counting period during which the number of pulses is counted in the block. In this case, the counting periods of the respective blocks start at the same time. A symbol .SIGMA. means that the signal pulses are counted for the period indicated in each block.
Referring to FIGS. 4 and 5, reference numeral 12 denotes a counting section, which includes counters 1a to 1f, latches 2a to 2f, counters 6a to 6f and latches 7a to 7c. Reference numeral 13 indicates a dynamic error calculating section, which is made up of a calculating section 3 and an integrating part 9. Respective gyros (not shown) provide gyro pulses .DELTA..phi..sub.x, .DELTA..phi..sub.y and .DELTA..phi..sub.z each generated per unit quantity of rotation about their axes. These pulses are fed to positive or negative terminals according to the direction of rotation.
The counter 1a is reset upon each completion of counting of the sequentially input gyro pulses .DELTA..phi..sub.x for the 20-msec counting period. During this period of time, the latch 2a latches the intermediate count value .DELTA..phi..sub.x with a period of 0.1 msec and provides it to the calculating section 3. The latch 4a latches every counting period of 20 msec the final count value .PHI..sub.x in that period and provides it to a strap-down calculation processor 5, and at the same time, it resets the counting.
In the counter 1b the gyro pulses .DELTA..phi..sub.x which are input thereinto are counted for a 0.1-msec counting period, and upon each completion of the counting period, the counting is reset. The latch 2a latches, every 0.1-msec period, the number of pulses, .omega..sub.x, counted by the counter 1b to the point of time immediately before its resetting, and the latch 2a provides the count value to the calculating section 3. Similarly, the gyro pulses .DELTA..phi..sub.y and .DELTA..phi..sub.z are also counted by the corresponding counters 1c to 1f for the 0.1-msec period, and the count values are latched by the latches 2c to 2f, from which they are provided to the calculating section 3 and the latches 4b and 4c.
Accelerometers on the respective axes provide pulses .DELTA.V.sub.x, .DELTA.V.sub.y and .DELTA.V.sub.z which are generated per unit magnitude of velocity variations in their axial direction. These acceleration pulses are applied to positive or negative terminals, depending upon whether the velocity is on increase or decrease.
A description will be given of the processing of the acceleration pulses .DELTA.V.sub.x. The counter 6a counts the pulses .DELTA.V.sub.x for the 0.1-msec counting period and resets the counting upon each completion of the period. Let the number of pulses counted during one counting period be represented by A.sub.x. The latch 7a of the next stage similarly fetches therein the pulse count value A.sub.x and provides the count value to the calculating section 3.
The counter 6b sequentially counts the acceleration pulses .DELTA.V.sub.x for the 20-msec computation period and is reset upon each completion of the period. Let the number of pulses counted during this period be represented by V.sub.x. A latch 8a of an output section 4 fetches therein the pulse count value V.sub.x every 20-msec computation period (in synchronization with the counter 6b) and provides the numerical value to the strap-down calculation processor 5. The other acceleration pulses .DELTA.V.sub.y and .DELTA.V.sub.z are also similarly processed by the counters 6c to 6f and latches 7b, 7c, 8b and 8c.
In multiplier/subtractors 3a to 3c of the calculating section 3 the calculations shown in the second and third terms on the right side of the aforementioned formula (1) are performed to obtain calculated values .DELTA.C.sub.A, .DELTA.C.sub.B and .DELTA.C.sub.C every 0.1 msec. In multiplier/subtractors 3g to 3i the calculations shown in the second and third terms on the right side of the formula (2) are performed to obtain calculated values .DELTA.S.sub.A, .DELTA.S.sub.B and .DELTA.S.sub.C every 0.1 msec. In accumulators 9a to 9c and 9g to 9i of the integrating section 9 the calculated values obtained every 0.1 msec are accumulated in each 20-msec computation period to obtain integrated values C.sub.A, C.sub.B, C.sub.C and S.sub.A, S.sub.B, S.sub.C as shown in the expressions (1a), (1b), (1c) and (2a), (2b), (2c), respectively. In latches 10a to 10c and 10g to 10i these integrated values C.sub.A, C.sub.B, C.sub.C and S.sub.A, S.sub.B, S.sub.C are latched with a period of 20 msec, and they are provided to the strap-down calculating processor 5. Thus, the strap-down calculating processor 5 performs dynamic error correcting calculations given by the following expressions: EQU .PHI..sub.x +C.sub.A, .PHI..sub.y +C.sub.B, .PHI..sub.z +C.sub.C EQU V.sub.x +S.sub.A, V.sub.y +S.sub.B, V.sub.z +S.sub.C.
FIGS. 6 and 7 show an arrangement for dynamic error correcting processing shown by formulae (7) and (8) in the case of using the (Y, Z, S)-axis sensor system. The parts corresponding to those in FIGS. 4 and 5 are identified by the same reference numerals. The example shown in FIGS. 6 and 7 employs gyro pulses .DELTA..phi..sub.y, .DELTA..phi..sub.z, .DELTA..phi..sub.s and acceleration pulses .DELTA.V.sub.y, .DELTA.V.sub.z, .DELTA.V.sub.s. Since the processing in this example is the same as that described above in respect of FIGS. 4 and 5, no description will be given thereof. Incidentally, the fourth terms of the formulae (7y), (7z), (8y) and (8z) are multiplied by a factor of .sqroot.3--this is effected in the strap-down calculating section 5. By this processing, the calculations of the above-mentioned formulae (7) and (8) are conducted. Also in the cases of the (X, Y, S)-axis and (X, Z, S)-axis sensor systems, processing corresponding to the formulae (3), ( 4) and (5), (6) are executed by arrangements (not shown) as in the above, and no description will be given of them.
In the event that the sensor on any one of the above-said axes has broken down or its performance has deteriorated, the calculations for navigation can be continued by use of sensors on the other three axes. However, the detection of such an abnormal sensor calls for the calculations involving all of the four sensor systems and dynamic error correcting calculations necessary therefor, that is, calculations of all of the formulae (1) to (8). Hence, large quantities of data to be processed for correction and high computation frequency are needed--this increases the load on the computer and enlarges the scale of the system.